/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <camera_detect/zzw.h>
#include <tf/transform_datatypes.h>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <fstream>


ros::Publisher all_obstacle_vis_pub;
// global variable
mavros_msgs::State uav_cur_state;
geometry_msgs::PoseStamped uav_cur_pos;
camera_detect::zzw small_aruco_pos;
double aruco_pos_x = 0, aruco_pos_y=0, aruco_pos_z=0;
bool aruco_isfind = false;
// marker for aruco pos vis
visualization_msgs::Marker aruco_marker,ob1,ob2,ob3,ob4,ob5;
visualization_msgs::MarkerArray all_obstacle_marker;
// CB function
void uav_state_cb(const mavros_msgs::State::ConstPtr& msg){
    uav_cur_state = *msg;
}

void uav_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    uav_cur_pos = *msg;
}


int main(int argc, char **argv)
{
    //    try to read point from file
    std::vector<Eigen::Vector3d> wayPoints;
    // Create an input filestream
    std::ifstream myFile("/home/zph/ros_ws/hgj_ws/src/path-points-smooth/build/curv.csv");
    // Make sure the file is open
    if(!myFile.is_open()) throw std::runtime_error("Could not open file");
    // Helper vars
    std::string line;
    double val;
    // Read data, line by line
    while(std::getline(myFile, line))
    {
        // Create a stringstream of the current line
        std::stringstream ss(line);
        // Keep track of the current column index
        int colIdx = 0;
        double x, y, z;
        // Extract each double
        while(ss >> val){
            // Add the current integer to the 'colIdx' column's values vector
            if(colIdx==0) x = val;
            if(colIdx==1) y = val;
            if(colIdx==2) z = val;
            // If the next token is a comma, ignore it and move on
            if(ss.peek() == ',') ss.ignore();
            // Increment the column index
            colIdx++;
        }
        Eigen::Vector3d v(x,y,z);
        wayPoints.insert(wayPoints.begin(),v);
    }

    // Close file
    myFile.close();
    //init ros
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    all_obstacle_vis_pub = nh.advertise<visualization_msgs::MarkerArray>( "all_obstacle_vis", 10 );
    ros::Subscriber uav_state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, uav_state_cb);
    ros::Subscriber uav_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("/mavros/local_position/pose", 2, uav_pos_cb);


    ros::Publisher offb_setpos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(200.0);

    // wait for FCU connection
    while(ros::ok() && !uav_cur_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    aruco_marker.header.frame_id = "map";
    aruco_marker.header.stamp = ros::Time();
    aruco_marker.ns = "aruco";
    aruco_marker.id = 0;
    aruco_marker.type = visualization_msgs::Marker::CUBE;
    aruco_marker.action = visualization_msgs::Marker::ADD;
    aruco_marker.pose.position.x = 10;
    aruco_marker.pose.position.y = 10;
    aruco_marker.pose.position.z = 2.0;
    aruco_marker.pose.orientation.x = 0.0;
    aruco_marker.pose.orientation.y = 0.0;
    aruco_marker.pose.orientation.z = 0.0;
    aruco_marker.pose.orientation.w = 1.0;
    aruco_marker.scale.x = 5;
    aruco_marker.scale.y = 5;
    aruco_marker.scale.z = 4;
    aruco_marker.color.a = 1.0; // Don't forget to set the alpha!
    aruco_marker.color.r = 0.0;
    aruco_marker.color.g = 0.0;
    aruco_marker.color.b = 1.0;

    ob1.header.frame_id = "map";
    ob1.header.stamp = ros::Time();
    ob1.ns = "aruco";
    ob1.id = 1;
    ob1.type = visualization_msgs::Marker::CUBE;
    ob1.action = visualization_msgs::Marker::ADD;
    ob1.pose.position.x = 5;
    ob1.pose.position.y = 40;
    ob1.pose.position.z = 8.0;
    ob1.pose.orientation.x = 0.0;
    ob1.pose.orientation.y = 0.0;
    ob1.pose.orientation.z = 0.0;
    ob1.pose.orientation.w = 1.0;
    ob1.scale.x = 5;
    ob1.scale.y = 5;
    ob1.scale.z = 16;
    ob1.color.a = 1.0; // Don't forget to set the alpha!
    ob1.color.r = 0.0;
    ob1.color.g = 0.0;
    ob1.color.b = 1.0;

    ob2.header.frame_id = "map";
    ob2.header.stamp = ros::Time();
    ob2.ns = "aruco";
    ob2.id = 2;
    ob2.type = visualization_msgs::Marker::CUBE;
    ob2.action = visualization_msgs::Marker::ADD;
    ob2.pose.position.x = 30;
    ob2.pose.position.y = 15;
    ob2.pose.position.z = 1.5;
    ob2.pose.orientation.x = 0.0;
    ob2.pose.orientation.y = 0.0;
    ob2.pose.orientation.z = 0.0;
    ob2.pose.orientation.w = 1.0;
    ob2.scale.x = 15;
    ob2.scale.y = 15;
    ob2.scale.z = 3;
    ob2.color.a = 1.0; // Don't forget to set the alpha!
    ob2.color.r = 0.0;
    ob2.color.g = 0.0;
    ob2.color.b = 1.0;

    ob3.header.frame_id = "map";
    ob3.header.stamp = ros::Time();
    ob3.ns = "aruco";
    ob3.id = 3;
    ob3.type = visualization_msgs::Marker::CUBE;
    ob3.action = visualization_msgs::Marker::ADD;
    ob3.pose.position.x = 30;
    ob3.pose.position.y = 45;
    ob3.pose.position.z = 5.0;
    ob3.pose.orientation.x = 0.0;
    ob3.pose.orientation.y = 0.0;
    ob3.pose.orientation.z = 0.0;
    ob3.pose.orientation.w = 1.0;
    ob3.scale.x = 15;
    ob3.scale.y = 5;
    ob3.scale.z = 10;
    ob3.color.a = 1.0; // Don't forget to set the alpha!
    ob3.color.r = 0.0;
    ob3.color.g = 0.0;
    ob3.color.b = 1.0;

    ob4.header.frame_id = "map";
    ob4.header.stamp = ros::Time();
    ob4.ns = "aruco";
    ob4.id = 4;
    ob4.type = visualization_msgs::Marker::CUBE;
    ob4.action = visualization_msgs::Marker::ADD;
    ob4.pose.position.x = 25;
    ob4.pose.position.y = 25;
    ob4.pose.position.z = 6.0;
    ob4.pose.orientation.x = 0.0;
    ob4.pose.orientation.y = 0.0;
    ob4.pose.orientation.z = 0.0;
    ob4.pose.orientation.w = 1.0;
    ob4.scale.x = 2;
    ob4.scale.y = 2;
    ob4.scale.z = 12;
    ob4.color.a = 1.0; // Don't forget to set the alpha!
    ob4.color.r = 0.0;
    ob4.color.g = 0.0;
    ob4.color.b = 1.0;

    ob5.header.frame_id = "map";
    ob5.header.stamp = ros::Time();
    ob5.ns = "aruco";
    ob5.id = 5;
    ob5.type = visualization_msgs::Marker::CUBE;
    ob5.action = visualization_msgs::Marker::ADD;
    ob5.pose.position.x = 25;
    ob5.pose.position.y = 25;
    ob5.pose.position.z = 10.0;
    ob5.pose.orientation.x = 0.0;
    ob5.pose.orientation.y = 0.0;
    ob5.pose.orientation.z = 0.0;
    ob5.pose.orientation.w = 1.0;
    ob5.scale.x = 10;
    ob5.scale.y = 10;
    ob5.scale.z = 8;
    ob5.color.a = 1.0; // Don't forget to set the alpha!
    ob5.color.r = 0.0;
    ob5.color.g = 1.0;
    ob5.color.b = 0.0;

    all_obstacle_marker.markers.push_back(aruco_marker);
    all_obstacle_marker.markers.push_back(ob1);
    all_obstacle_marker.markers.push_back(ob2);
    all_obstacle_marker.markers.push_back(ob3);
    all_obstacle_marker.markers.push_back(ob4);
    all_obstacle_marker.markers.push_back(ob5);
    all_obstacle_vis_pub.publish(all_obstacle_marker);
    //send a few setpoints before starting
    // Before entering Offboard mode,
    // you must have already started streaming setpoints.
    // Otherwise the mode switch will be rejected.
    geometry_msgs::PoseStamped pos_before_offb;
    for(int i = 10; ros::ok() && i > 0; --i){
        pos_before_offb = uav_cur_pos;
        offb_setpos_pub.publish(pos_before_offb);
        ros::spinOnce();
        rate.sleep();
    }

    auto cur_point = wayPoints.back();
    wayPoints.pop_back();
    while(ros::ok()){

        if(uav_cur_state.armed){
            if(uav_cur_state.mode == "OFFBOARD"){
                double delt_x = uav_cur_pos.pose.position.x- cur_point.x();
                double delt_y = uav_cur_pos.pose.position.y- cur_point.y();
                double dis_to_target_point = sqrt(delt_x*delt_x+delt_y*delt_y);
                double safe_dis = 0.5;
                if (dis_to_target_point>safe_dis){
                    geometry_msgs::PoseStamped pose;
                    pose.pose.position.x = cur_point.x();
                    pose.pose.position.y = cur_point.y();
                    pose.pose.position.z = 2;
                    offb_setpos_pub.publish(pose);
                    ROS_INFO_THROTTLE(1, "adjusting");
                    pos_before_offb = uav_cur_pos;
                }
                else{
                    if(!wayPoints.empty()){
                        cur_point = wayPoints.back();
                        wayPoints.pop_back();
                        geometry_msgs::PoseStamped pose;
                        pose.pose.position.x = cur_point.x();
                        pose.pose.position.y = cur_point.y();
                        pose.pose.position.z = 2;
                        offb_setpos_pub.publish(pose);
                        ROS_INFO_THROTTLE(1, "adjusting");
                        pos_before_offb = uav_cur_pos;
                    }
                    else{
                        offb_setpos_pub.publish(pos_before_offb);
                    }


                }

            }
            else{
                ROS_INFO_THROTTLE(2, "waiting for Offboard cmd");
                offb_setpos_pub.publish(uav_cur_pos);
                pos_before_offb = uav_cur_pos;
            }
        }
        else{
            ROS_INFO_THROTTLE(2, "waiting for Vehicle arm");
            offb_setpos_pub.publish(uav_cur_pos);
            pos_before_offb = uav_cur_pos;
            all_obstacle_vis_pub.publish(all_obstacle_marker);

        }

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}